import numpy as np
from myMapminmax import myMapminmax
from checkX import checkX


def aimFcn_1(x, option, data):
    """
    计算路径规划问题的适应度函数
    参数:
        x: 决策变量数组
        option: 算法参数配置
        data: 环境数据字典
    返回:
        fit: 适应度值
        result: 包含路径和适应度的结果字典
    """
    x0 = x.copy()
    num_e0 = len(data['E0'])
    
    # 提取终点排序参数并排序
    x2 = x[:num_e0]
    x = x[num_e0:]
    sorted_indices = np.argsort(x2)
    E0_sorted = data['E0'][sorted_indices]
    
    # 使用地形数据计算默认高度偏移
    
    pathR = []
    pn = 0  # 终点高度差惩罚累计
    
    # 构建巡检路径
    PR = [data['noS']] + [data['noE0'][i] for i in sorted_indices]
    
    for noP in range(len(E0_sorted)):
        if noP == 0:
            nowP_idx = PR[noP]
            aimE_idx = PR[noP + 1]
            now_xy = data['S'][:2]  # 使用实际起点坐标而非节点坐标
            now_h = data['S'][2]  # 使用起点实际高度
        else:
            nowP_idx = PR[noP]
            aimE_idx = PR[noP + 1]
            now_xy = data['node'][nowP_idx - 1, :2]
            now_h = path[-1, 3]  # 使用上一段路径终点高度
        
        # 初始化路径记录
        path = np.array([[nowP_idx, now_xy[0], now_xy[1], now_h, 0, 0]])
        flag = np.zeros(data['numNode'], dtype=int)
        flag[nowP_idx - 1] = 1  # 标记已访问节点
        
        # 路径搜索主循环
        while nowP_idx != aimE_idx:
            # 查找相邻节点并计算到目标的距离
            neighbors = data['net'][data['net'][:, 0] == nowP_idx, 1]
            target_node = data['node'][aimE_idx - 1, :2]  # 终点坐标
            
            # 过滤未访问节点并按距离目标排序
            unvisited = []
            distances = []
            for n in neighbors:
                if flag[n - 1] == 0:
                    node_xy = data['node'][n - 1, :2]
                    dist_to_target = np.sqrt(np.sum((node_xy - target_node)**2))
                    unvisited.append(n)
                    distances.append(dist_to_target)
            
            # 按距离目标升序排序（优先选择更近的节点）
            if unvisited:
                sorted_indices = np.argsort(distances)
                unvisited = [unvisited[i] for i in sorted_indices]
            if not unvisited:
                # 回溯到上一节点
                nowP_idx = int(path[-2, 0])
                now_h = path[-2, 3]
                path = path[:-1]
                continue
            
            # 计算相邻节点评估指标
            eval_metrics = []
            for n in unvisited:
                node_data = data['node'][n - 1]
                x_n, y_n = node_data[:2]
                
                # 计算目标高度 - 初始化默认值
                target_h = data['minH']
                if n == aimE_idx:
                    # 初始化目标高度默认值 (使用4空格缩进确保一致性)
                    target_h = E0_sorted[noP, 2]
                    # 动态调整目标高度，增加高度平滑过渡权重
                    if len(path) > 1:
                        prev_h = path[-1, 3]
                        # 计算水平距离并限制高度变化率
                        dx = node_data[0] - path[-1, 1]
                        dy = node_data[1] - path[-1, 2]
                        dist = np.sqrt(dx**2 + dy**2)
                        max_h_change = 0.5 * dist
                        target_h = np.clip(node_data[2] + 5, prev_h - max_h_change, prev_h + max_h_change)
                    target_h = max(node_data[2] + 5, data['minH'])






                
                # 高度限制检查
                dist = np.linalg.norm([x_n, y_n] - now_xy)
                target_h = np.clip(target_h, now_h - dist, now_h + dist)
                target_h = max(target_h, node_data[2] + 0.5)
                
                # 计算方向连续性指标
            # 计算方向连续性指标
            if len(path) > 1:
                prev_dir = path[-1, 1:3] - path[-2, 1:3]
                curr_dir = [x_n, y_n] - now_xy
                if np.linalg.norm(prev_dir) > 0 and np.linalg.norm(curr_dir) > 0:
                    # 计算方向夹角余弦值（-1到1，1表示同向）
                    dir_cosine = np.dot(prev_dir, curr_dir) / (np.linalg.norm(prev_dir) * np.linalg.norm(curr_dir))
                    direction_cost = 1 - dir_cosine  # 方向变化成本（0到2）
                else:
                    direction_cost = 0
            else:
                direction_cost = 0

            # 计算评估指标，增加方向连续性权重
            height_gap = abs(now_h - target_h)
            total_dist = dist + np.linalg.norm([x_n, y_n] - E0_sorted[noP, :2])
            priority = x0[n - 1] if n - 1 < len(x0) else np.inf
            eval_metrics.append([height_gap, total_dist, priority, direction_cost * 0.5])
            
            # 多目标优化选择最优节点，增加随机探索
            metrics_array = np.array(eval_metrics)
            # 归一化所有评估指标（包括新增的方向成本）
            normalized = np.array([myMapminmax(metrics_array[:, i]) for i in range(4)]).T
            combined = np.sum(normalized, axis=1)
            
            # 10%概率随机选择，增加探索能力
            if np.random.rand() < 0.1 and len(combined) > 1:
                # 排除最差选项的随机选择
                worst_idx = np.argmax(combined)
                available_indices = [i for i in range(len(combined)) if i != worst_idx]
                best_idx = np.random.choice(available_indices)
            else:
                best_idx = np.argmin(combined)
            
            # 更新当前状态
            nowP_idx = unvisited[best_idx]
            now_h = max(data['node'][nowP_idx - 1, 2] + 1, data['minH'])
            flag[nowP_idx - 1] = 1
            
            # 更新路径记录
            new_xy = data['node'][nowP_idx - 1, :2]
            segment_dist = np.linalg.norm(new_xy - now_xy)
            height_gap = abs(now_h - path[-1, 3])
            path = np.vstack([path, [nowP_idx, new_xy[0], new_xy[1], now_h, segment_dist, height_gap]])
            now_xy = new_xy
        
        # 累计终点高度差惩罚
        pn += abs(E0_sorted[noP, 2] - now_h)
        pathR.append(path)
    
    # 计算总适应度
    total_dist = np.sum([p[:, 4].sum() for p in pathR])
    total_height_gap = np.sum([p[:, 5].sum() for p in pathR])
    fit = total_dist + pn
    
    # 准备返回结果
    result = {'fit': fit, 'path': pathR}
    return result